#include "segment.h"

float cac_angle(std::vector<float> v1, std::vector<float> v2)
{
    float costheta = (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])/(pow(pow(v1[0],2)+pow(v1[1],2)+pow(v1[2],2),0.5)
                    +pow(pow(v2[0],2)+pow(v2[1],2)+pow(v2[2],2),0.5));
    float theta = acos(costheta)*180/3.14;
    return theta;
}

pcl::ModelCoefficients::Ptr Plane_model_segment(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out, pcl::PointIndices::Ptr inliers, bool option)
{
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setOptimizeCoefficients(option);
    seg.setMaxIterations (500);
    seg.setDistanceThreshold(0.01);    
    seg.setInputCloud(cloud_in);
    seg.segment(*inliers, *coefficients);
    //决定是否去除该平面的内点
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(cloud_in);
    extract.setIndices(inliers);
    extract.setNegative(option);
    extract.filter(*cloud_out);
    return coefficients;
}

/**
 * @brief Get the forward plane object
 *      从点云中提取出弹药箱正面
 * @param cloud 
 * @return pcl::PointIndices::Ptr 
 */
pcl::PointCloud<pcl::PointXYZ>::Ptr get_forward_plane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsample)
{
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);

    //提取前景平面
    pcl::ModelCoefficients::Ptr coefficients = Plane_model_segment(cloud_downsample, result, inliers, true);

    //获取前景平面法向量
    std::vector<float> v_main{coefficients->values[0], coefficients->values[1], coefficients->values[2]};
    
    //再次提取平面
    coefficients = Plane_model_segment(result, result, inliers, false);

    //搜索kd tree
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (result);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;   //欧式聚类对象
    ec.setClusterTolerance (0.02);                     // 设置近邻搜索的搜索半径为2cm
    ec.setMinClusterSize (50);                 //设置一个聚类需要的最少的点数目为50
    ec.setMaxClusterSize (25000);               //设置一个聚类需要的最大点数目为25000
    ec.setSearchMethod (tree);                    //设置点云的搜索机制
    ec.setInputCloud (result);
    ec.extract (cluster_indices);           //从点云中提取聚类，并将点云索引保存在cluster_indices中

    //迭代访问索引直到分割出所有聚类
    int max_size = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    {
        float sum = 0;
        for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
        {
            cloud_cluster->points.push_back (result->points[*pit]);
            sum = result->points[*pit].x + result->points[*pit].y + result->points[*pit].z;
        }
        cloud_cluster->width = cloud_cluster->points.size ();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;
        if(max_size < cloud_cluster->width)
        {
            max_size = cloud_cluster->width;
            result = cloud_cluster;
        }
    }
    return result;
}

/*
void test_code()
{
    //搜索kd tree
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud_downsample);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;   //欧式聚类对象
    ec.setClusterTolerance (0.02);                     // 设置近邻搜索的搜索半径为2cm
    ec.setMinClusterSize (50);                 //设置一个聚类需要的最少的点数目为50
    ec.setMaxClusterSize (25000);               //设置一个聚类需要的最大点数目为25000
    ec.setSearchMethod (tree);                    //设置点云的搜索机制
    ec.setInputCloud (cloud_downsample);
    ec.extract (cluster_indices);           //从点云中提取聚类，并将点云索引保存在cluster_indices中

    //迭代访问索引直到分割出所有聚类
    int i = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    {
        float sum = 0;
        for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
        {
            cloud_cluster->points.push_back (cloud_downsample->points[*pit]);
            sum = cloud_downsample->points[*pit].x + cloud_downsample->points[*pit].y + cloud_downsample->points[*pit].z;
        }
        cloud_cluster->width = cloud_cluster->points.size ();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        float angle;
        do{
            //每个聚类进行平面拟合
            pcl::ModelCoefficients::Ptr coefficients = Plane_model_segment(cloud_cluster, inliers, true);
            std::vector<float> v1{coefficients->values[0],coefficients->values[1],coefficients->values[2]};
            //计算该聚类                                            
            //将拟合后的内点输出
            angle = cac_angle(v1,v_main);
            std::cerr<<angle<<std::endl;
            extract.setInputCloud(cloud_cluster);
            extract.setIndices(inliers);
            extract.setNegative(true);
            extract.filter(*cloud_cluster);
        }while(angle>10);
        //if(cac_angle(v1,v_main)<10)
        //{
        //    result = 
        //}
                                               
    }
}
*/ 